#include "postprocess/type_fuser/sequence_typefuser.h"
#include "common/ad_logger.h"

SequenceTypeFuser::SequenceTypeFuser() : s_alpha_(1.2f) {}

SequenceTypeFuser::~SequenceTypeFuser() {}

void SequenceTypeFuser::Init(int type_count) {
  A_ = Eigen::MatrixXf::Zero(type_count, type_count);
  // 这是原始值
  //   A_ << 9.95905989e-01, 1.59211524e-03,
  //   0.00000000e+00, 7.58150114e-05, 6.82335102e-04,
  //   0.00000000e+00, 6.06520091e-04,
  //   0.00000000e+00, 3.03260045e-04, 8.33965125e-04,

  //       3.20821303e-03, 9.94225217e-01, 0.00000000e+00,
  //       0.00000000e+00, 1.12287456e-03,
  //       0.00000000e+00, 1.60410651e-04, 1.60410651e-04, 1.60410651e-04, 9.62463908e-04,

  //       0.00000000e+00, 0.00000000e+00, 9.91735537e-01, 0.00000000e+00,
  //       0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 8.26446281e-03,

  //       5.58659218e-03, 0.00000000e+00, 0.00000000e+00, 9.94413408e-01,
  //       0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 0.00000000e+00,

  //       1.74737893e-03, 2.49625562e-03, 0.00000000e+00,
  //       0.00000000e+00, 9.95756365e-01, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 0.00000000e+00, 0.00000000e+00,

  //       0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 9.37500000e-01, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 6.25000000e-02,

  //       5.19287834e-03, 7.41839763e-04, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 0.00000000e+00, 9.91839763e-01, 0.00000000e+00,
  //       0.00000000e+00, 2.22551929e-03,

  //       0.00000000e+00, 4.95049505e-03, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 0.00000000e+00, 4.95049505e-03, 9.90099010e-01,
  //       0.00000000e+00, 0.00000000e+00,

  //       1.27551020e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
  //       0.00000000e+00, 9.82142857e-01, 5.10204082e-03,

  //       9.72689862e-04, 2.99289188e-04, 7.48222970e-05,
  //       0.00000000e+00, 7.48222970e-05, 1.49644594e-04, 7.48222970e-05, 7.48222970e-05,
  //       1.49644594e-04, 9.98129443e-01;

  // 优化之后的值
  A_ << 0.85, 0.02, 0.01, 0.02, 0.02, 0.01, 0.02, 0.01, 0.02, 0.02,

      0.02, 0.85, 0.01, 0.01, 0.02, 0.01, 0.02, 0.02, 0.02, 0.02,

      0.01, 0.01, 0.9, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.02,

      0.02, 0.01, 0.01, 0.9, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01,

      0.02, 0.02, 0.01, 0.01, 0.89, 0.01, 0.01, 0.01, 0.01, 0.01,

      0.01, 0.01, 0.01, 0.01, 0.01, 0.9, 0.01, 0.01, 0.01, 0.02,

      0.02, 0.02, 0.01, 0.01, 0.01, 0.01, 0.88, 0.01, 0.01, 0.02,

      0.01, 0.02, 0.01, 0.01, 0.01, 0.01, 0.02, 0.89, 0.01, 0.01,

      0.03, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.88, 0.02,

      0.02, 0.02, 0.02, 0.01, 0.02, 0.02, 0.02, 0.02, 0.02, 0.83;
  for (int i = 0; i < type_count; ++i) {
    for (int j = 0; j < type_count; ++j) {
      A_(i, j) = log(A_(i, j));  // 以e为底
    }
  }
  s_alpha_ = 1.2f;
}

int SequenceTypeFuser::FuseType() {
  std::size_t length = fused_oneshot_probs_.size();
  fused_sequence_probs_.resize(length);
  fused_sequence_probs_[0] = fused_oneshot_probs_[0];
  fused_sequence_probs_[0] +=
      A_.row(0).transpose();  // 这里是ln，加法相当于乘法
  // 这里应该是乘以对角阵，获得初值
  //   for (std::size_t k = 1; k < VALID_OBJECT_TYPE; ++k) {
  //     fused_sequence_probs_[0](k) += A_(k, k);
  //   }

  //   std::cout << "---------------------FuseType--------------------------- "
  //             << std::endl;
  for (std::size_t i = 1; i < length; ++i) {
    // std::cout << "length =  " << length << " i= " << i << std::endl;
    for (std::size_t right = 0; right < VALID_OBJECT_TYPE; ++right) {
      float max_prob = -FLT_MAX;
      std::size_t id = 0;
      for (std::size_t left = 0; left < VALID_OBJECT_TYPE; ++left) {
        const float prob = fused_sequence_probs_[i - 1](left) +
                           A_(left, right) * s_alpha_ +
                           fused_oneshot_probs_[i](right);

        if (prob > max_prob) {
          max_prob = prob;
          id = left;
        }
      }
      fused_sequence_probs_[i](right) = max_prob;
    }
  }

  std::vector<float> dst_prob;
  ToExp(&fused_sequence_probs_.back());
  Normalize(&fused_sequence_probs_.back());
  FromEigenVector(fused_sequence_probs_.back(), &dst_prob);
  int type = static_cast<int>(std::distance(
      dst_prob.begin(), std::max_element(dst_prob.begin(), dst_prob.end())));
  //   std::cout << "type = " << type << std::endl;
  return type;
}

void SequenceTypeFuser::AddObjectProbs(TrackedObjectPtr* new_object) {
  if (fused_oneshot_probs_.size() > HISTORY_CCRF_FRAME) {
    fused_oneshot_probs_.pop_front();
  }
  Vectorf vec;
  for (int i = 0; i < VALID_OBJECT_TYPE; ++i) {
    vec(i) = log((*new_object)->scores[i]);
  }
  fused_oneshot_probs_.push_back(vec);

  //   ADLOGGER_INFO("fused_oneshot_probs_ data...");
  //   for (size_t j = 0; j < fused_oneshot_probs_.size(); j++) {
  //     const Vectorf& vec = fused_oneshot_probs_.at(j);
  //     std::string str;
  //     for (int k = 0; k < VALID_OBJECT_TYPE; k++) {
  //       const float& a = vec(k, 1);
  //       str += std::to_string(a) + " ";
  //     }
  //     ADLOGGER_INFO(str);
  //   }
}

void SequenceTypeFuser::ToExp(Vectorf* prob) {
  double min_value = prob->minCoeff();
  for (std::size_t i = 0; i < VALID_OBJECT_TYPE; ++i) {
    (*prob)(i) = exp((*prob)(i)-min_value);
  }
}

void SequenceTypeFuser::Normalize(Vectorf* prob) {
  double sum = prob->sum();
  sum = sum < 1e-9 ? 1e-9 : sum;
  *prob /= sum;
}

void SequenceTypeFuser::FromEigenVector(const Vectorf& src_prob,
                                        std::vector<float>* dst_prob) {
  dst_prob->assign(VALID_OBJECT_TYPE, 0);
  dst_prob->at(0) = src_prob(0);
  for (std::size_t i = 0; i < VALID_OBJECT_TYPE; ++i) {
    dst_prob->at(i) = static_cast<float>(src_prob(i));
  }
}